#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
#include "roboard_drivers/Encoder.h"
#include "roboard_drivers/Motor.h"
#include "roboard_drivers/serial_adc_val.h"
#include "roboard_drivers/enable_adc_val.h"
#include "roboard_drivers/servo.h"
#include "roboard_drivers/digitalIO.h"
#include <string.h>
#include "serializer/Serializer.cc"
#include <iostream>
#include <pthread.h>

using namespace roboard_drivers;
Serializer * s;
ros::Subscriber sub1;
ros::Subscriber sub2;
ros::Subscriber sub3;
ros::Subscriber sub4;
ros::Subscriber sub5;
ros::Subscriber sub6;
ros::Subscriber sub7;
ros::Subscriber sub8;

ros::Publisher  enc_pub;
ros::Publisher  adc_pub;
ros::Publisher  dio_pub;

pthread_mutex_t mutex_serial = PTHREAD_MUTEX_INITIALIZER;


int encoder_interval	= 200;
int adc_interval 		= 200;
int dio_interval 		= 200;

bool read_adc0			= true;
bool read_adc1			= true;
bool read_adc2			= true;
bool read_adc3			= true;
bool read_adc4			= true;

bool read_dio0			= false;
bool read_dio1			= false;
bool read_dio2			= false;
bool read_dio3			= false;
bool read_dio4			= false;
bool read_dio5			= false;
bool read_dio6			= false;
bool read_dio7			= false;
bool read_dio8			= false;
bool read_dio9			= false;

//Callback function for the "/encoder_interval" topic. As the name indicates this will change the rate at which the "/encoder" topic is published.
void change_encoder_interval(const std_msgs::Int32::ConstPtr &msg)
{
	encoder_interval = msg->data;
	printf("serializer:encoder_interval: %i\n",msg->data);
}

void change_adc_interval(const std_msgs::Int32::ConstPtr &msg)
{
	adc_interval = msg->data;
	printf("serializer:adc_interval: %i\n",msg->data);
}

void change_dio_interval(const std_msgs::Int32::ConstPtr &msg)
{
	adc_interval = msg->data;
	printf("serializer:dio_interval: %i\n",msg->data);
}

void enable_adc(const enable_adc_val::ConstPtr &msg)
{
	read_adc0			= msg->enable0;
	read_adc1			= msg->enable1;
	read_adc2			= msg->enable2;
	read_adc3			= msg->enable3;
	read_adc4			= msg->enable4;
}

void enable_read_dio(const digitalIO::ConstPtr &msg)
{
	read_dio0			= bool(msg->val0);
	read_dio1			= bool(msg->val1);
	read_dio2			= bool(msg->val2);
	read_dio3			= bool(msg->val3);
	read_dio4			= bool(msg->val4);
	read_dio5			= bool(msg->val5);
	read_dio6			= bool(msg->val6);
	read_dio7			= bool(msg->val7);
	read_dio8			= bool(msg->val8);
	read_dio9			= bool(msg->val9);
}

void set_dio(const digitalIO::ConstPtr &msg)
{
	std::vector< std::pair<int,bool> > cmd;
	if(msg->val0 == 1){cmd.push_back(std::make_pair(0,true));}
	else if(msg->val0 == 0){cmd.push_back(std::make_pair(0,false));}

	if(msg->val1 == 1){cmd.push_back(std::make_pair(1,true));}
	else if(msg->val1 == 0){cmd.push_back(std::make_pair(1,false));}

	if(msg->val2 == 1){cmd.push_back(std::make_pair(2,true));}
	else if(msg->val2 == 0){cmd.push_back(std::make_pair(2,false));}

	if(msg->val3 == 1){cmd.push_back(std::make_pair(3,true));}
	else if(msg->val3 == 0){cmd.push_back(std::make_pair(3,false));}

	if(msg->val4 == 1){cmd.push_back(std::make_pair(4,true));}
	else if(msg->val4 == 0){cmd.push_back(std::make_pair(4,false));}

	if(msg->val5 == 1){cmd.push_back(std::make_pair(5,true));}
	else if(msg->val5 == 0){cmd.push_back(std::make_pair(5,false));}

	if(msg->val6 == 1){cmd.push_back(std::make_pair(6,true));}
	else if(msg->val6 == 0){cmd.push_back(std::make_pair(6,false));}

	if(msg->val7 == 1){cmd.push_back(std::make_pair(7,true));}
	else if(msg->val7 == 0){cmd.push_back(std::make_pair(7,false));}

	if(msg->val8 == 1){cmd.push_back(std::make_pair(8,true));}
	else if(msg->val8 == 0){cmd.push_back(std::make_pair(8,false));}

	if(msg->val9 == 1){cmd.push_back(std::make_pair(9,true));}
	else if(msg->val9 == 0){cmd.push_back(std::make_pair(9,false));}

	pthread_mutex_lock( &mutex_serial );
	s->setIO(cmd);
	pthread_mutex_unlock( &mutex_serial );
	
}

void * dio_thread_func( void *ptr ){
	ros::Rate loop_rate(1000);
	struct timeval start, end;
	while(ros::ok()){
		gettimeofday(&start, NULL);
		std::vector< std::pair<int,bool> > query;

		if(read_dio0){
			std::pair<int,bool> p;
			p.first = 0;
			query.push_back(p);
		}
		if(read_dio1){
			std::pair<int,bool> p;
			p.first = 1;
			query.push_back(p);
		}
		if(read_dio2){
			std::pair<int,bool> p;
			p.first = 2;
			query.push_back(p);
		}
		if(read_dio3){
			std::pair<int,bool> p;
			p.first = 3;
			query.push_back(p);
		}
		if(read_dio4){
			std::pair<int,bool> p;
			p.first = 4;
			query.push_back(p);
		}
		if(read_dio5){
			std::pair<int,bool> p;
			p.first = 5;
			query.push_back(p);
		}
		if(read_dio6){
			std::pair<int,bool> p;
			p.first = 6;
			query.push_back(p);
		}
		if(read_dio7){
			std::pair<int,bool> p;
			p.first = 7;
			query.push_back(p);
		}
		if(read_dio8){
			std::pair<int,bool> p;
			p.first = 8;
			query.push_back(p);
		}

		if(read_dio9){
			std::pair<int,bool> p;
			p.first = 9;
			query.push_back(p);
		}

		if(query.size() > 0){
			int delay = 1;
			pthread_mutex_lock( &mutex_serial );
			while(!s->getIO(query)){
				pthread_mutex_unlock( &mutex_serial );				
				usleep(1000);
				delay++;
				pthread_mutex_lock( &mutex_serial );
			}
			pthread_mutex_unlock( &mutex_serial );
			gettimeofday(&end, NULL);

			digitalIO msg;
			msg.timestamp = end.tv_sec+double(end.tv_usec)/1000000.0;
			msg.val0 = -1;
			msg.val1 = -1;
			msg.val2 = -1;
			msg.val3 = -1;
			msg.val4 = -1;
			msg.val5 = -1;
			msg.val6 = -1;
			msg.val7 = -1;
			msg.val8 = -1;
			msg.val9 = -1;
			for(unsigned int i = 0; i < query.size(); i++)
			{
				int ind = query.at(i).first;
				int val = query.at(i).second; 
				switch(ind){
					case(0):{msg.val0 = val;break;}
					case(1):{msg.val1 = val;break;}
					case(2):{msg.val2 = val;break;}
					case(3):{msg.val3 = val;break;}
					case(4):{msg.val4 = val;break;}
					case(5):{msg.val5 = val;break;}
					case(6):{msg.val6 = val;break;}
					case(7):{msg.val7 = val;break;}
					case(8):{msg.val8 = val;break;}
					case(9):{msg.val9 = val;break;}
				}
			}
			dio_pub.publish(msg);
		}
		
		ros::spinOnce();
		gettimeofday(&end, NULL);
		while((end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec)) < dio_interval*1000){
			loop_rate.sleep();
			gettimeofday(&end, NULL);
		}
		ros::spinOnce();
	}
}

void change_motor_speed(const Motor::ConstPtr &msg)
{
	printf("serializer:change motor speed: L: %f , R:%f\n",msg->left,msg->right);
	double l = 100*msg->left;
	double r = 100*msg->right;
	int delay = 1;
	if(l > 100){l = 100;}
	if(l < -100){l = -100;}

	if(r > 100){r = 100;}
	if(r < -100){r = -100;}
	pthread_mutex_lock( &mutex_serial );
	while(!s->setMotorPWMs(l,r)){
		pthread_mutex_unlock( &mutex_serial );
		usleep(delay*1000);
		delay++;
		pthread_mutex_lock( &mutex_serial );
		}
	pthread_mutex_unlock( &mutex_serial );
}

void move_servo(const servo::ConstPtr &msg){
	int id = msg->id;
	int angle = msg->angle;
	std::vector< std::pair<int,int> > cmd;
    cmd.push_back(std::make_pair(id,angle));
	
	pthread_mutex_lock( &mutex_serial );
    s->setServoPos(cmd);
	pthread_mutex_unlock( &mutex_serial );
}
//bool setServoPos(std::vector< std::pair<int, int> > &cmd);

void * enc_thread_func( void *ptr ){
	ros::Rate loop_rate(1000);
	struct timeval start, end;
	while(ros::ok()){
		gettimeofday(&start, NULL);
		long r = 0;
		long l = 0;
		//s->getEncoders(r, l);
		int delay = 1;
		pthread_mutex_lock( &mutex_serial );
		while(!s->getEncoders(r, l)){
			pthread_mutex_unlock( &mutex_serial );
			usleep(delay*1000);
			delay++;
			pthread_mutex_lock( &mutex_serial );		
		}
		pthread_mutex_unlock( &mutex_serial );

		Encoder msg;
		msg.right 	= r;
		msg.left 	= l;
		msg.timestamp = start.tv_sec+double(start.tv_usec)/1000000.0;
		enc_pub.publish(msg);
		
		ros::spinOnce();
		gettimeofday(&end, NULL);
		while((end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec)) < encoder_interval*1000){
			loop_rate.sleep();
			gettimeofday(&end, NULL);
		}
		ros::spinOnce();
	}
}

void * adc_thread_func( void *ptr ){

	ros::Rate loop_rate(1000);
	struct timeval start, end;
	while(ros::ok()){
		gettimeofday(&start, NULL);
		std::vector< std::pair<int,int> > query;

		if(read_adc0){
			std::pair<int,int> p;
			p.first = 0;
			p.second = -1;
			query.push_back(p);
		}
		if(read_adc1){
			std::pair<int,int> p;
			p.first = 1;
			p.second = -1;
			query.push_back(p);
		}
		if(read_adc2){
			std::pair<int,int> p;
			p.first = 2;
			p.second = -1;
			query.push_back(p);
		}
		if(read_adc3){
			std::pair<int,int> p;
			p.first = 3;
			p.second = -1;
			query.push_back(p);
		}
		if(read_adc4){
			std::pair<int,int> p;
			p.first = 4;
			p.second = -1;
			query.push_back(p);
		}

		if(query.size() > 0){
			int delay = 1;
			pthread_mutex_lock( &mutex_serial );
			while(!s->readADC(query)){
				pthread_mutex_unlock( &mutex_serial );				
				usleep(1000);
				delay++;
				pthread_mutex_lock( &mutex_serial );
			}
			pthread_mutex_unlock( &mutex_serial );
			gettimeofday(&end, NULL);
			serial_adc_val msg;
			msg.timestamp = end.tv_sec+double(end.tv_usec)/1000000.0;
			msg.val0 = -1;
			msg.val1 = -1;
			msg.val2 = -1;
			msg.val3 = -1;
			msg.val4 = -1;
			for(unsigned int i = 0; i < query.size(); i++)
			{
				int ind = query.at(i).first;
				int val = query.at(i).second; 
				switch(ind){
					case(0):{msg.val0 = val;break;}
					case(1):{msg.val1 = val;break;}
					case(2):{msg.val2 = val;break;}
					case(3):{msg.val3 = val;break;}
					case(4):{msg.val4 = val;break;}
				}
			}
			adc_pub.publish(msg);
		}
		
		ros::spinOnce();
		gettimeofday(&end, NULL);
		while((end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec)) < adc_interval*1000){
			loop_rate.sleep();
			gettimeofday(&end, NULL);
		}
		ros::spinOnce();
	}
}

int main(int argc, char **argv)
{
	s = new Serializer();
	s->connect("/dev/ttyS2");
	s->setUnit(Serializer::UNIT_RAW);
	s->setEncoderType(Serializer::ENCODER_QUAD);
	s->clearEncoders();
	
	ros::init(argc, argv, "serial");
	ros::NodeHandle n;
	
	enc_pub = n.advertise<Encoder>("/serial/encoder"	, 100000);
	pthread_t encoder_thread;
	pthread_create( &encoder_thread, NULL, enc_thread_func, NULL);
	
	adc_pub = n.advertise<serial_adc_val>("/serial/adc", 100000);
	pthread_t adc_thread;
	pthread_create( &adc_thread, NULL, adc_thread_func, NULL);

	dio_pub = n.advertise<digitalIO>("/serial/digitalIO", 100000);
	pthread_t dio_thread;
	pthread_create( &dio_thread, NULL, dio_thread_func, NULL);
	
	
	sub1 	= n.subscribe("/serial/encoder_interval"	, 1, change_encoder_interval);
	sub2 	= n.subscribe("/serial/motor_speed"		, 1, change_motor_speed);
	sub3 	= n.subscribe("/serial/adc_interval", 1, change_adc_interval);
	sub4 	= n.subscribe("/serial/enable_adc", 1, enable_adc);
	sub5 	= n.subscribe("/serial/move_servo", 1, move_servo);
	sub6 	= n.subscribe("/serial/digitalIO_interval", 1, change_dio_interval);
	sub7 	= n.subscribe("/serial/digitalIO_set", 1, set_dio);
	sub8 	= n.subscribe("/serial/digitalIO_read", 1, enable_read_dio);

	ros::spin();
	usleep(500000);
	s->stopAllMotors();
	s->disconnect();
	return 0;
}
